/**
* @file distance.c
 *
 * @section desc File description
 *
 * @section copyright Copyright
 *
 * Trampoline is copyright (c) IRCCyN 2005-2007
 * Trampoline is protected by the French intellectual property law.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; version 2
 * of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 * @section infos File informations
 *
 * $Date$ 2013 04 18
 * $Rev$ 1
 * $Author$ Benjamin Sientzoff
 * $URL$
 */


/* Libraries */
#include "tpl_os.h"
#include "nxt_motors.h"
#include "ecrobot_interface.h"
#include "ecrobot_private.h"
#include "com_protocol.h"

SINT bluetooth_status = BT_NO_INIT;
boolean bt_connected = FALSE;
extern U8 bt_frame[128];
extern boolean bt_frame_received;
//TODO: set bt_send_buffer size matching with max data send
U8 bt_mail[128] = {0, 0, 0, 0};

U8 chunk[256];
S16 dist_data[1];
U8 motorCount=0;
boolean frappe=FALSE;

// What is the length of a string ?
/* strlen function from string.h */
U8 stringlength(char* s) 
{
		/* find length of s[] */
		U8 i = 0;
		while (s[i] != '\0'){
				i++;
		}
		return i;
}
/* Prototypes */
void init_genericI2C_sensor(U8 port_id);
void get_mindsensor_distance_sensor(U8 port_id, S16 *buf);
void term_genericI2C_sensor(U8 port_id);
void update_distance();


void update_distance() {
		get_mindsensor_distance_sensor(NXT_PORT_S3, dist_data);
		systick_wait_ms(2000);

}


void init_genericI2C_sensor(U8 port_id)
{
	nxt_avr_set_input_power(port_id,2);
	i2c_enable(port_id);
}

void get_mindsensor_distance_sensor(U8 port_id, S16 *buf)
{
    static S16 distance_state[0];
    static U8 data[2] = {0,0};
	/*
	 * Data spec. of mindsensor ultrasound sensor
	 *
	 * 0x42 data[0]: Distance data LSB
	 * 0x43 data[1]: Distance data MSB
	 */
    
	if (i2c_busy(port_id) == 0) /* check the status of I2C comm. */
	{
		
        distance_state[0] = ((S16)data[1]<<8) + (S16)data[0];
        
        /* i2c_start_transaction just triggers an I2C transaction,
         * actual data transaction between ARM7 and an Ultrasound
         * Sensor is done by an ISR after this, so there is one execution cycle
         * delay for consistent data acquistion
         */
		i2c_start_transaction(port_id,1,0x42,1,data,9,0);
	}
	
    buf[0] = distance_state[0];
    
	
}

void term_genericI2C_sensor(U8 port_id)
{
	i2c_disable(port_id);
}


// "prettyprint" for Pong robot screen
// Print a statement on the last screen line
void print(char* statement)
{
		/*if (17<stringlength(statement)) {
				display_goto_xy(0,5);
				for (int i = 0; i<stringlength(statement); i++) {
						if (i == 15)
						{
								display_goto_xy(0,6);
						}
						else if (i == 30) {
								display_goto_xy(0,7);
						}
						display_string(statement[i]);
				}
		}
else {
				display_goto_xy(0,5);
				display_string(statement); 
}*/
		display_goto_xy(0,7);
		display_string(statement);
		display_update();
}



/* Main */
FUNC(int,OS_APPL_CODE) main(void)
{            
		StartOS(OSDEFAULTAPPMODE);
	
		return 0;
}
/* Hooks */
FUNC(void,OS_APPL_CODE) StartupHook(void)
{
	init_genericI2C_sensor(NXT_PORT_S1);
}

FUNC(void,OS_APPL_CODE) ShutdownHook(StatusType error)
{
	term_genericI2C_sensor(NXT_PORT_S1);

}

/* TASKS */

//Task launched when Trampoline starts
TASK(init)
{
		
		TerminateTask();
}
TASK(majDistance){
	
	bluetooth_status = ecrobot_get_bt_status();
	update_distance();	

	
	TerminateTask();
}
//Display several informations about system,motors and sensors
TASK(display)
{
		display_clear(0);
		//Battery stat
   	display_goto_xy(0,0);
   	display_string("PWR:");
   	display_unsigned(ecrobot_get_battery_voltage()*100/9000,0);
				
		//System time
   	display_goto_xy(8,0);
   	display_string("CLK:");
   	display_unsigned(systick_get_ms()/1000,0);
		
		//Bluetooth info
   	display_goto_xy(0,1);
   	display_string("BT:");
		bluetooth_status = ecrobot_get_bt_status();
		//Bluetooth off
		if (BT_NO_INIT == bluetooth_status) {
				display_goto_xy(3,1);
				display_string("OFF");
		}
		//Bluetooth off
		else if (BT_INITIALIZED == bluetooth_status) {
				display_goto_xy(3,1);
				display_string("ON");
		}
		else if (BT_CONNECTED == bluetooth_status) {
				display_goto_xy(3,1);
				display_string("LNK");
		}
		//Reception/Sending data 
		else if (BT_STREAM == bluetooth_status) {
				display_goto_xy(3,1);
				display_string("R/S");
		}
		else {
				display_goto_xy(3,1);
				display_int(bluetooth_status,2);
		}
		if (BT_NO_INIT != bluetooth_status) {

				display_goto_xy(0,2);
				display_string("FRAME:");
				display_goto_xy(1,3);
				display_int(bt_frame[0],3);
				display_goto_xy(5,3);
				display_int(bt_frame[1],3);
				display_goto_xy(9,3);
				display_int(bt_frame[2],3);
				display_goto_xy(13,3);
				display_int(bt_frame[3],3);
		}
		
	display_goto_xy(0, 5);
	display_string("I2C Test");
	display_goto_xy(0, 6);
	display_string("Distance :");
	display_int(dist_data[0], 0);
    
    // Get count et affiche
    motorCount=nxt_motor_get_count(NXT_PORT_B);
    display_goto_xy(0, 7);
    //display_int( motorCount,3);
    
    //Calcul distance
    get_mindsensor_distance_sensor(NXT_PORT_S3, dist_data);
	display_int( dist_data[0],3);
	switch (bt_frame[1]) {
		case 0x00:
			ecrobot_set_motor_speed(NXT_PORT_A, 0);
			break;
		case 0x02:
			ecrobot_set_motor_speed(NXT_PORT_A, 100);
			break;
		case 0x04:
			ecrobot_set_motor_speed(NXT_PORT_A, -100);
			break;
		case 0x08:
			while (ecrobot_get_touch_sensor(NXT_PORT_S2) !=1){
				ecrobot_set_motor_speed(NXT_PORT_B, -100);
			}
			//while ((ecrobot_get_touch_sensor(NXT_PORT_S1) !=1) && (motorCount != nxt_motor_get_count(NXT_PORT_B)) ) {
            //while ((ecrobot_get_touch_sensor(NXT_PORT_S1) !=1)  ) {
          /*  ecrobot_set_motor_speed(NXT_PORT_B, 40);
            nxt_motor_set_count(0,NXT_PORT_B);
            while(motorCount < 220){
                systick_wait_ms(50) ; //On attend
                if (motorCount== nxt_motor_get_count(NXT_PORT_B)){
                    motorCount = 250;
                }
                else {
                    motorCount=nxt_motor_get_count(NXT_PORT_B);
                }
                ecrobot_set_motor_speed(NXT_PORT_B, 40);


			}*/
             ecrobot_set_motor_speed(NXT_PORT_B, 40);
            nxt_motor_set_count(0,NXT_PORT_B);
            motorCount== nxt_motor_get_count(NXT_PORT_B);
            boolean continuer=true;
            U8 watchdog=0;
           // while ((ecrobot_get_touch_sensor(NXT_PORT_S1) !=1)  ) {
            while (continuer){
                systick_wait_ms(50) ;//On attend 50 ms
                if (ecrobot_get_touch_sensor(NXT_PORT_S1) ==1 ){
                    continuer =false;
                }
                if (watchdog > 20 ){
                     continuer =false;
                    while (ecrobot_get_touch_sensor(NXT_PORT_S2) !=1){
                        ecrobot_set_motor_speed(NXT_PORT_B, -50);
                    }

                   
                }
                motorCount=nxt_motor_get_count(NXT_PORT_B);
                
                ecrobot_set_motor_speed(NXT_PORT_B, 40);
                watchdog++;
                

            
            }
            
			ecrobot_set_motor_speed(NXT_PORT_B, 0);

			// Un seule coup
			bt_frame[1]=0;
            
            
          
			break;
		default:
			ecrobot_set_motor_speed(NXT_PORT_A, 100);
			
			break;
	}
   /* if (  frappe == true) {
          frappe = false;
        while (ecrobot_get_touch_sensor(NXT_PORT_S2) !=1){
            ecrobot_set_motor_speed(NXT_PORT_B, -100);
        }
        while (ecrobot_get_touch_sensor(NXT_PORT_S1) !=1){
            ecrobot_set_motor_speed(NXT_PORT_B, 40);
        }
        ecrobot_set_motor_speed(NXT_PORT_B, 0);
        
    }*/
	
	
	// End moteur
	
		display_update();
		TerminateTask();
	//ActivateTask(majDistance);
}

//Send data throught bluetooth connection
TASK(postman)
{
	/*	bt_mail[0] = bluetooth_status;
	    bt_mail[1] = 1;
		bt_mail[2] = 2;
		bt_mail[3] = 3;
	*//*	
	display_goto_xy(0, 5);
	display_int(bluetooth_status,3);
	display_goto_xy(0, 6);
*/
	/*ecrobot_send_bt_packet(bt_mail, 128);
		
	display_int(bluetooth_status,3);
	display_int(dist_data[0], 0);
	display_update();*/
/*while (BT_CONNECTED != bluetooth_status) {
		bluetooth_status = ecrobot_get_bt_status();

	}*/


	
		//ActivateTask(majDistance);
/*	ecrobot_send_bt_packet(bt_mail, 128);
	get_mindsensor_distance_sensor(NXT_PORT_S3, dist_data);
	//systick_wait_ms(20);
	
	//MOTOR
	switch (bt_frame[1]) {
		case 0x00:
			ecrobot_set_motor_speed(NXT_PORT_A, 0);
			break;
		case 0x02:
			ecrobot_set_motor_speed(NXT_PORT_A, 100);
			break;
		case 0x04:
			ecrobot_set_motor_speed(NXT_PORT_A, -100);
			break;
		case 0x08:
			while (ecrobot_get_touch_sensor(NXT_PORT_S1) !=1){
				ecrobot_set_motor_speed(NXT_PORT_B, -100);	
			}
			while (ecrobot_get_touch_sensor(NXT_PORT_S2) !=1){
				ecrobot_set_motor_speed(NXT_PORT_B, 60);	
			}
			ecrobot_set_motor_speed(NXT_PORT_B, 0);	

			break;
		default:
			ecrobot_set_motor_speed(NXT_PORT_A, 100);

			break;
	}
	
	*/
		TerminateTask();
}

/* ISRs */
ISR(isr_button_stop)
{
	
	/*while (1){
		

		ecrobot_set_motor_speed(NXT_PORT_A, 100);
	}*/
    ShutdownOS(E_OK);
}

ISR(isr_button_start)
{
	

	ecrobot_set_motor_speed(NXT_PORT_B, 0);
		if (bt_connected) {
				display_clear(0);
				display_goto_xy(0,3);
				display_string("Already");
				display_goto_xy(5,4);
				display_string("connected!");
				display_update();
				systick_wait_ms(2000);
		}
		else {
				display_clear(0);
				display_goto_xy(0,3);
				display_string("Connecting ...");
				display_update();
				// Connect to bluetooth master
				/*while(FALSE == bt_connected) {
						ecrobot_init_bt_slave("ROBOT2");
					//Attention la commande suivante demonte la liaison BT
					// Peut ^tre à faire avant le init A tester
					// ou faire un programme spécifique pour 
					// fixer le nom de la brique
					
					//ecrobot_set_bt_device_name("ROBOT2");
						if ((BT_STREAM == ecrobot_get_bt_status()) && (BT_STREAM != bluetooth_status)) {
								bt_connected = TRUE;
						}
							bluetooth_status = ecrobot_get_bt_status();
				}*/
            bt_frame[1] = 0x08;
                
		}
		ActivateTask(display);
}

ISR(isr_bluetooth)
{
		if(bt_frame_received) {
				print("BT: recep");
				ecrobot_read_bt_packet(bt_frame, 128);
			//	ActivateTask(postman);
		}
		else {
				print("BT: ????");
		}
	// Moteur
	
	
	
}